{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 09 从URDF/SDF文件创建机器人\n",
    "\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 运行演示\n",
    "\n",
    "### 创建工作空间并获取模型\n",
    "\n",
    "```\n",
    "cd drake\n",
    "mkdir -p examples/double_pendulum_pid/models\n",
    "curl -o examples/double_pendulum_pid/models/double_pendulum.sdf https://raw.githubusercontent.com/guzhaoyuan/drake/tutorial/examples/double_pendulum_pid/models/double_pendulum.sdf\n",
    "```\n",
    "\n",
    "操作时下载失败，所以从guzhaoyuan的drake克隆的仓库中将对应的模型内容拷贝到/drake/examples/中\n",
    "\n",
    "\n",
    "### 创建源码文件\n",
    "\n",
    "```\n",
    "BUILD.bazel\n",
    "load(\n",
    "    \"@drake//tools/skylark:drake_cc.bzl\",\n",
    "    \"drake_cc_binary\",\n",
    ")\n",
    "load(\"//tools/install:install_data.bzl\", \"install_data\")\n",
    "\n",
    "drake_cc_binary(\n",
    "    name = \"run_double_pendulum_passive_exe\",\n",
    "    srcs = [\n",
    "        \"run_double_pendulum_passive.cc\",\n",
    "    ],\n",
    "    data = [\n",
    "        \":models\",\n",
    "        \"//tools:drake_visualizer\",\n",
    "    ],\n",
    "    deps = [\n",
    "        \"//common:find_resource\",\n",
    "        \"//common:text_logging_gflags\",\n",
    "        \"//geometry:geometry_visualization\",\n",
    "        \"//lcm\",\n",
    "        \"//multibody/parsing\",\n",
    "        \"//multibody/plant\",\n",
    "        \"//systems/analysis\",\n",
    "        \"//systems/framework\",\n",
    "        \"@sdformat\",\n",
    "    ],\n",
    ")\n",
    "\n",
    "install_data()\n",
    "```\n",
    "\n",
    "\n",
    "\n",
    "```\n",
    "///\n",
    "/// @brief  An SDF based double pendulum example.\n",
    "///\n",
    "\n",
    "#include <gflags/gflags.h>\n",
    "\n",
    "#include \"drake/common/drake_assert.h\"\n",
    "#include \"drake/common/find_resource.h\"\n",
    "#include \"drake/common/text_logging_gflags.h\"\n",
    "#include \"drake/geometry/geometry_visualization.h\"\n",
    "#include \"drake/geometry/scene_graph.h\"\n",
    "#include \"drake/lcm/drake_lcm.h\"\n",
    "#include \"drake/multibody/parsing/parser.h\"\n",
    "#include \"drake/multibody/plant/multibody_plant.h\"\n",
    "#include \"drake/systems/analysis/simulator.h\"\n",
    "#include \"drake/systems/framework/diagram.h\"\n",
    "#include \"drake/systems/framework/diagram_builder.h\"\n",
    "\n",
    "DEFINE_double(target_realtime_rate, 1.0,\n",
    "              \"Rate at which to run the simulation, relative to realtime\");\n",
    "DEFINE_double(simulation_time, 10, \"How long to simulate the pendulum\");\n",
    "DEFINE_double(max_time_step, 1.0e-3,\n",
    "              \"Simulation time step used for integrator.\");\n",
    "\n",
    "namespace drake {\n",
    "namespace examples {\n",
    "namespace double_pendulum {\n",
    "namespace {\n",
    "\n",
    "// Fixed path to double pendulum SDF model.\n",
    "static const char* const kDoublePendulumSdfPath =\n",
    "    \"drake/examples/double_pendulum_pid/models/double_pendulum.sdf\";\n",
    "\n",
    "//\n",
    "// Main function for demo.\n",
    "//\n",
    "void DoMain() {\n",
    "  DRAKE_DEMAND(FLAGS_simulation_time > 0);\n",
    "\n",
    "  systems::DiagramBuilder<double> builder;\n",
    "\n",
    "  geometry::SceneGraph<double>& scene_graph =\n",
    "      *builder.AddSystem<geometry::SceneGraph>();\n",
    "  scene_graph.set_name(\"scene_graph\");\n",
    "\n",
    "  // Load and parse double pendulum SDF from file into a tree.\n",
    "  multibody::MultibodyPlant<double>* dp =\n",
    "      builder.AddSystem<multibody::MultibodyPlant<double>>(FLAGS_max_time_step);\n",
    "  dp->set_name(\"plant\");\n",
    "  dp->RegisterAsSourceForSceneGraph(&scene_graph);\n",
    "\n",
    "  multibody::Parser parser(dp);\n",
    "  const std::string sdf_path = FindResourceOrThrow(kDoublePendulumSdfPath);\n",
    "  multibody::ModelInstanceIndex plant_model_instance_index =\n",
    "      parser.AddModelFromFile(sdf_path);\n",
    "  (void)plant_model_instance_index;\n",
    "\n",
    "  // Weld the base link to world frame with no rotation.\n",
    "  const auto& root_link = dp->GetBodyByName(\"base\");\n",
    "  dp->AddJoint<multibody::WeldJoint>(\"weld_base\", dp->world_body(), nullopt,\n",
    "                                     root_link, nullopt,\n",
    "                                     Isometry3<double>::Identity());\n",
    "  // Now the plant is complete.\n",
    "  dp->Finalize();\n",
    "\n",
    "  // Connect plant with scene_graph to get collision information.\n",
    "  DRAKE_DEMAND(!!dp->get_source_id());\n",
    "  builder.Connect(\n",
    "      dp->get_geometry_poses_output_port(),\n",
    "      scene_graph.get_source_pose_port(dp->get_source_id().value()));\n",
    "  builder.Connect(scene_graph.get_query_output_port(),\n",
    "                  dp->get_geometry_query_input_port());\n",
    "\n",
    "  geometry::ConnectDrakeVisualizer(&builder, scene_graph);\n",
    "\n",
    "  auto diagram = builder.Build();\n",
    "  std::unique_ptr<systems::Context<double>> diagram_context =\n",
    "      diagram->CreateDefaultContext();\n",
    "\n",
    "  // Create plant_context to set velocity.\n",
    "  systems::Context<double>& plant_context =\n",
    "      diagram->GetMutableSubsystemContext(*dp, diagram_context.get());\n",
    "\n",
    "  // Set init position.\n",
    "  Eigen::VectorXd positions = Eigen::VectorXd::Zero(2);\n",
    "  positions[0] = 0.1;\n",
    "  positions[1] = 0.4;\n",
    "  dp->SetPositions(&plant_context, positions);\n",
    "\n",
    "  systems::Simulator<double> simulator(*diagram, std::move(diagram_context));\n",
    "  simulator.set_publish_every_time_step(true);\n",
    "  simulator.set_target_realtime_rate(FLAGS_target_realtime_rate);\n",
    "  simulator.Initialize();\n",
    "  simulator.AdvanceTo(FLAGS_simulation_time);\n",
    "}\n",
    "\n",
    "}  // namespace\n",
    "}  // namespace double_pendulum\n",
    "}  // namespace examples\n",
    "}  // namespace drake\n",
    "\n",
    "int main(int argc, char** argv) {\n",
    "  gflags::SetUsageMessage(\n",
    "      \"bazel run \"\n",
    "      \"//examples/double_pendulum_pid:run_double_pendulum_passive_exe\");\n",
    "  gflags::ParseCommandLineFlags(&argc, &argv, true);\n",
    "  drake::examples::double_pendulum::DoMain();\n",
    "  return 0;\n",
    "}\n",
    "```\n"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.11"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
